clear all
close all
global k
global r
handle = @kleinstekwadraten;

n = 5;
p = 20*rand([2,n])-10;

d = zeros(n,n);

hold on
scatter(p(1,:), p(2,:),'filled')

for i=1:n;
    for j=1:n;
       d(i,j) = sqrt((p(1,j)-p(1,i))^2+(p(2,j)-p(2,i))^2); 
    end
end

for i=1:n;
    r = d(i,1:i-1);
    if i==1;
        k(:,i) = [0;0];
    elseif i==2;
        k(:,i) = [d(1,i);0];
    elseif i==3;
        k(:,i) = [((d(1,3)^2-d(2,3)^2+d(1,2)^2)/(2*d(1,2)));sqrt(d(1,3)^2-((d(1,3)^2-d(2,3)^2+d(1,2)^2)/(2*d(1,2)))^2)];
    else
        k(:,i) = lsqnonlin(handle, [0,0]);
    end
end

scatter(k(1,:), k(2,:),'filled')
pos = k;
position = k;
q = zeros(2,n);
d = zeros(1,n);

for t=0:pi/10:2*pi;
    for i=1:n;
        q(:,i) = [position(1,i)+cos(t);position(2,i)+sin(t)];
        for j=1:n;
            d(j) = sqrt((pos(1,j)-q(1,i))^2+(pos(2,j)-q(2,i))^2); 
        end
        d(i) = 0;
        r = d(1,:);
        x0 = pos(:,i)';
        k = pos;
        coords = k;
        v = r;
        pos = calculation(coords,v);
        k = pos;
    end
    scatter(k(1,:),k(2,:))
end
axis equal
hold off
shg